/* 
 * uart4IMU.c - The C source file of IMU(uart) devices
 * NOTE: This file is based on HAL library of stm32 platform
 *          PWM设备BSP接口实现定义
 *          接口声明功能注释见BSP.h
 *
 * Copyright (c) 2020-, FOSH Project
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author            Notes           mail
 * 2021-03-04     江榕煜       first version   2373180028@qq.com
**/

#include "IMU_WT931.h"
#include "BSP.h"

#ifdef __USER_DEBUG__   // 全局调试打印使能

#define __USER_DEBUG__IMU__

#endif
//#define __USER_DEBUG__IMU__ //手动启动单项调试打印

#define IMU_MODE_ANALYSE_IN_DMAINT  // 在DMA中断中解析数据

#ifndef IMU_MODE_ANALYSE_IN_DMAINT
#define IMU_MODE_ANALYSE_BY_USER    // 用户进行采样触发和解析
#else
volatile static int32_t uartDMA4IMU_status = 0;
#endif

//#define IMU_UART_HANDLER huart3

#ifdef IMU_MODE_ANALYSE_IN_DMAINT
#define IMU_UART_BUFFER_LENGTH 2    // 理论上只要1个帧大小缓存，此处为了防止内存越界
#else
#define IMU_UART_BUFFER_LENGTH 6
static struct rt_semaphore imuRxSem;
#endif

volatile WT931_9AxisData imuData = {0};

volatile static uint8_t imuRecvCache[11*IMU_UART_BUFFER_LENGTH]; // 数据接收缓冲池
//static uint8_t imuSendCache[10];

/** @brief  IMU数据单次采样
 *  @note   这是一个运算为主要任务的函数，没有等待
 *  @author 江榕煜（2021.3.19）
 *  @param  None
 *  @retval None
**/
void IMU_Sampling(void)
{
    #ifdef IMU_MODE_ANALYSE_BY_USER // 用户采样，解析
    /* 开始接收一组数据接收 */
    __HAL_UART_CLEAR_FLAG(&IMU_UART_HANDLER, UART_FLAG_PE);//清标志
	__HAL_UART_CLEAR_FLAG(&IMU_UART_HANDLER, UART_FLAG_FE);
	__HAL_UART_CLEAR_FLAG(&IMU_UART_HANDLER, UART_FLAG_NE);
	__HAL_UART_CLEAR_FLAG(&IMU_UART_HANDLER, UART_FLAG_ORE);
    IMU_UART_HANDLER.Instance->RDR;
    HAL_UART_Receive_DMA(&IMU_UART_HANDLER,imuRecvCache,11*IMU_UART_BUFFER_LENGTH);
    /* 等待数据接收完成 */
    rt_sem_take(&imuRxSem, RT_WAITING_FOREVER);
    
    /* ---开始解析数据--- */
    #ifdef __USER_DEBUG__IMU__
    rt_kprintf("recvCache content:\n");
    for(int i = 0;i <80; i++)
        rt_kprintf("%x",imuRecvCache[i]);
    rt_kprintf("\n");
    #endif
    /* 寻找数据头 */
    for(int i = 0;
            i <= 11;
            i++)   // 只检查一个包范围的数据偏移量，若没有则可能是没有接收到有效数据
    {
        if(imuRecvCache[i] == 0x55) //检查到帧头
        {
            /* 打印台提示 */
            #ifdef __USER_DEBUG__IMU__
            rt_kprintf("search a frame head,offset:%d\n",i);
            #endif
            /* 解析数据 */
            for(int j = i;      //从最早的数据包开始
                j <= 11*(IMU_UART_BUFFER_LENGTH-1)-1; //防止内存访问越界
                j+=11)          // 一个有效数据包的长度为11
            {
                
                #ifdef __USER_DEBUG__IMU__
                rt_kprintf("analys content:\n");
                for(int k = 0;k <11; k++)
                    rt_kprintf("%x:",imuRecvCache[j+k]);
                rt_kprintf("\n");
                #endif
                WT931_9AxisDataAnalys(&imuData,imuRecvCache+j); //数据提取
            }
            break;  // 退出检查
        }
    }
    memset(imuRecvCache,0,80);  //内存清空，防止下一次误读
    return;
    #endif
}

#ifdef __USER_DEBUG__IMU__
MSH_CMD_EXPORT(IMU_Sampling,samp IMU);
#endif

/** @brief  IMU初始化
 *  @note   调用前先初始化串口
 *  @author 江榕煜（2021.3.19）
 *  @param  None
 *  @retval 
 *      WT931_9AxisData*    IMU数据对象（可直接读取）
**/
WT931_9AxisData * IMU_Init(void)
{
    #ifdef IMU_MODE_ANALYSE_IN_DMAINT
    // 需要将传输配置为在DMA中断中解析数据
    // 所以要做字节序对齐工作
    
    /* 开始接收一组数据接收 */
    __HAL_UART_CLEAR_FLAG(&IMU_UART_HANDLER, UART_FLAG_PE);//清标志
	__HAL_UART_CLEAR_FLAG(&IMU_UART_HANDLER, UART_FLAG_FE);
	__HAL_UART_CLEAR_FLAG(&IMU_UART_HANDLER, UART_FLAG_NE);
	__HAL_UART_CLEAR_FLAG(&IMU_UART_HANDLER, UART_FLAG_ORE);
    IMU_UART_HANDLER.Instance->RDR;
    HAL_UART_Receive_DMA(&IMU_UART_HANDLER,imuRecvCache,11*IMU_UART_BUFFER_LENGTH);
    
    uartDMA4IMU_status = 0; // 标志未对齐
    while(uartDMA4IMU_status != 2) ; // 如果没有进入对齐状态，则一直卡死在初始化里
    #endif
    
    #ifdef IMU_MODE_ANALYSE_BY_USER
    // 初始化接收成功信号量
    rt_sem_init(&imuRxSem,"imuRxSem",0,0);
    #endif
    
    // 返回imu对象
    return &imuData;
}

#ifdef __USER_DEBUG__IMU__
/** @brief  IMU数据打印
 *  @note   测试用
 *  @author 江榕煜（2021.3.19）
 *  @param  None
 *  @retval None
**/
int IMU_print()
{
    rt_kprintf("--------------------\n");
    rt_kprintf("the time is: %d:%d:%d\n",
                imuData.Time.ucHour,
                imuData.Time.ucMinute,
                imuData.Time.ucSecond);
    rt_kprintf("the accelerate is: %d,%d,%d\n",
                imuData.Accelerate.a[0],
                imuData.Accelerate.a[1],
                imuData.Accelerate.a[2]);
    rt_kprintf("the angle is: %d,%d,%d\n",
                imuData.Angle.Angle[0],
                imuData.Angle.Angle[1],
                imuData.Angle.Angle[2]);
    rt_kprintf("the gyro is: %d,%d,%d\n",
                imuData.Gyro.w[0],
                imuData.Gyro.w[1],
                imuData.Gyro.w[2]);
    rt_kprintf("--------------------\n");
  
    return 0;
}

MSH_CMD_EXPORT(IMU_print, print the data of IMU);
#endif

/** @brief  上位机接收回调函数
 *  @note   收到一个完整帧，目前没有做自动字节序对齐
 *  @author 江榕煜（2021.6.15）
 *  @param  huart(UART_HandleTypeDef *) 串口对象
 *  @retval None
**/
void UART4IMU_RxCpltCallback(UART_HandleTypeDef *huart)
//void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
    if(huart == &IMU_UART_HANDLER)
    {
        #ifdef IMU_MODE_ANALYSE_BY_USER
        rt_sem_release(&imuRxSem);
        #endif
        
        #ifdef IMU_MODE_ANALYSE_IN_DMAINT
        switch(uartDMA4IMU_status)
        {
            case(0): // 刚开始匹配
            {
                // 先计算偏移量
                /* 寻找数据头 */
                for(int i = 0;
                        i < 11;
                        i++)   // 只检查一个包范围的数据偏移量，若没有则可能是没有接收到有效数据
                {
                    if(imuRecvCache[i] == 0x55 && imuRecvCache[i+11] == 0x55) //检查到帧头
                    {
                        if(i == 0)  // 无偏移，对准良好
                        {
                            uartDMA4IMU_status = 2;
                            // 以正常包大小接收
                            HAL_UART_Receive_DMA(&IMU_UART_HANDLER,imuRecvCache,11);
                            return;
                        }
                        else
                        {
                            uartDMA4IMU_status = 1;
                            HAL_UART_Receive_DMA(&IMU_UART_HANDLER,imuRecvCache,11+i);
                            return;
                        }
                    }
                }
                HAL_UART_Receive_DMA(&IMU_UART_HANDLER,imuRecvCache,11*IMU_UART_BUFFER_LENGTH);
                return;
            }
            case(1):    // 过渡模式
            {
                uartDMA4IMU_status = 0; // 回到模式0，检验是否对齐
                HAL_UART_Receive_DMA(&IMU_UART_HANDLER,imuRecvCache,11*IMU_UART_BUFFER_LENGTH);
                return;
            }
            case(2):    // 正常模式
            {
                /* 在该模式下，正常解析数据 */
                if(imuRecvCache[0] == 0x55)
                {
                    WT931_9AxisDataAnalys(&imuData,imuRecvCache); //数据提取
                    HAL_UART_Receive_DMA(&IMU_UART_HANDLER,imuRecvCache,11);
                    return;
                }
                else
                {
                    uartDMA4IMU_status = 0; // 回到模式0，检验是否对齐
                    HAL_UART_Receive_DMA(&IMU_UART_HANDLER,imuRecvCache,11*IMU_UART_BUFFER_LENGTH);
                    return;
                }
            }
            
        }
        #endif
    }
}
